/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "PositionFileReader.hpp"
#include "owndebug.h"
int main(void){
  CPositionFileReader reader;
  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(1);

  //if(reader.ReadFile("J2B2.MaCI_Position.Motion_07_02_2011_16:41:32.txt")){
  if(reader.ReadFile("FSRSim.FSRSim_j2b2_morsko.MaCI_Position.Motion_10_03_2011_15:59:44.txt")){
    dPrint(1,"Succesfully read file.");

  }else{
    dPrint(1,"Failed to read file");
    exit(1);
  }


  std::list<gim::time> times;
  if(reader.GetTimes(times)){
    dPrint(1,"Got times... number of times %d",times.size());
  }else{
    dPrint(1,"Failed to get times ");
    exit(1);
  }


  TPositionDataMap map;
  if(reader.GetAllData(map)){
    dPrint(1,"Got all data. Size of the map = %d",map.size());
  }else{
    dPrint(1,"Failed to get all the data");
    exit(1);
  }
  
  MaCI::Position::CPositionData data;
  std::list<gim::time>::iterator iterFirst= times.begin();
  if(reader.GetPositionData(data,*iterFirst)){
    dPrint(1,"Got first position data..");
    data.Print(1);
  }else{
    dPrint(1,"Failed to get the first data");
    exit(1);
  }

  std::list<gim::time>::iterator iterLast = times.end();
  iterLast--;
  if(reader.GetPositionData(data,*iterLast)){
    dPrint(1,"Got last position data..");
    data.Print(1);
  }else{
    dPrint(1,"Failed to get the last data at %u %u",iterLast->seconds,iterLast->useconds)
    exit(1);
  }

  MaCI::Position::CPositionData poseData;
  dPrint(1,"***************************************************************************************");
  dPrint(1,"Trying to get the first data, by calling getpositiondata(CPositionData) at time %u %u",iterFirst->seconds,iterFirst->useconds);
  if(reader.GetPositionData(poseData,
                            *iterFirst)){
    dPrint(1,"Got first position data..");
    data.Print(1);

  }else{
    dPrint(1,"Failed to get the first data");
    exit(1);
  }

  TPositionLogLine logline;
  dPrint(1,"***************************************************************************************");
  dPrint(1,"Trying to get the first data, by calling getpositiondata(TPositionLogLine) at time %u %u",iterFirst->seconds,iterFirst->useconds);
  if(reader.GetPositionData(logline,
                            *iterFirst)){
    dPrint(1,"Got %u %u %f %f %f %f %f %f %f %f %f",
           logline.timestamp_s,
           logline.timestamp_us,
           logline.positionX,
           logline.positionY,
           logline.heading,
           logline.varianceX,
           logline.varianceY,
           logline.varianceHeading,
           logline.probability,
           logline.speed,
           logline.angularSpeed);
    
  }else{
    dPrint(1,"Failed to get the first data");
    exit(1);
  }
  dPrint(1,"***************************************************************************************");
  dPrint(1,"Trying to get the first data, by calling getpositiondata( MaCI::Position::TPose2D) at time %u %u",iterFirst->seconds,iterFirst->useconds);
  MaCI::Position::TPose2D pose;
  if(reader.GetPositionData(pose,
                            *iterFirst)){
    dPrint(1,"Got pose %f %f %f",
           pose.x,
           pose.y,
           pose.a);
       
  }else{
    dPrint(1,"Failed to get the first data");
    exit(1);
  }

  MaCI::Position::TSpeed2D speed;
  MaCI::Position::TVariance2D variance;
  MaCI::Position::TProbability probability;
  dPrint(1,"***************************************************************************************");
  dPrint(1,"Trying to get the first data, by calling getpositiondata( MaCI::Position::TPose2D,MaCI::Position::TSpeed2D,MaCI::Position::TVariance2D,MaCI::Position::TProbability) at time %u %u",iterFirst->seconds,iterFirst->useconds);
  if(reader.GetPositionData(pose,
                            speed,
                            variance,
                            probability,
                            *iterFirst)){
    dPrint(1,"Got pose %f %f %f, variance %f %f %f, probability %f, speed %f %f",
           pose.x,
           pose.y,
           pose.a,
           variance.x,
           variance.y,
           variance.a,
           probability.probability,
           speed.speed,
           speed.angularSpeed);

  }else{
    dPrint(1,"Failed to get the first data");
    exit(1);
  }

  dPrint(1,"***************************************************************************************");
  dPrint(1,"***************************************************************************************");
  dPrint(1,"***************************************************************************************");
  dPrint(1,"***************************************************************************************");
  dPrint(1,"Test ended succesfully. Congratulations!!! :) ");
  dPrint(1,"***************************************************************************************");
  dPrint(1,"***************************************************************************************");
  dPrint(1,"***************************************************************************************");

}
